clear all;
%% Глобальные константы
% Количество отметок
global g_N; g_N = 50;

% Минимальное число точек в кластере
global g_L; g_L = floor(g_N/10);

global DimX; global DimY;
DimX = 10e3; DimY = 10e3;
global Jx; global Jy; global J;
Jx = 20; Jy = 20; J = Jx*Jy;

% FIXME:
global g_DispMult; g_DispMult = 0.1;

% Отладочные флаги
global SHOW_SCENE; SHOW_SCENE = 1;
global SHOW_HYST; SHOW_HYST = 1;
global SHOW_EST_PDF; SHOW_EST_PDF = 1;
global ADD_NON_MIXTURE; ADD_NON_MIXTURE = 1;
global DO_QUANTIFY; DO_QUANTIFY = 1;
global DEBUG_LEVEL; DEBUG_LEVEL = 1;

%% Генерирование отсчетов (в виде смеси распределений)
% Координаты измеряются в м, скорости в м/с
Sigma = 1000;
CoordsObjectSigma = Sigma*diag(ones(2,1));
CoordsMean = [ ...
    5000 5000; ...
    5500 5500; ...
    5400 5000; ...
%    4900 4900
    ];
objNumber = length(CoordsMean);
CoordsCov = [];
for obj = 1:objNumber
  CoordsCov = cat(3, CoordsCov, CoordsObjectSigma);
end
CoordsP = ones(1, objNumber)/objNumber;
MixturePDF = gmdistribution(CoordsMean, CoordsCov, CoordsP);
Coords = random(MixturePDF, g_N);

%% Добавление отсчетов, не входящих в смесь
if ADD_NON_MIXTURE == 1
    Coords = add_non_mix_coords(10, Coords, [5000 5000], [40 40], [0 0]);
end

% Discritization
if DO_QUANTIFY == 1
    Coords = quantify(Coords, 100, 1e-2);
end

%% �?нициализация 
S = floor(g_N/10);
g = 1/S*ones(g_N, S);
w = 1/S*ones(S, 1);
phiCoord = Coords(1:S, :);
phiSigma = Sigma;

phiCov = [];
for s = 1:S
    phiCov = cat(3, phiCov, [phiSigma 0; 0 phiSigma]);
end

v = 1;
B = 0;
I = 100;
C = chi2inv(0.01, J - 3);
StartChiI = floor(I/10);
eps = 1e-4;

coordsA = @(j)[...
    rem(j - 1,Jx)*DimX/Jx ... % x1
    floor((j - 1)/Jx)*DimY/Jy; ... % y1
    (rem(j - 1, Jx) + 1)*DimX/Jx ... % x2
    floor((j - 1)/Jx + 1)*DimY/Jy]; % y2

SuspiciousCoords = [];

%% SEM-алгоритм
for i= 1:I
    %% Проверка условия выхода
    if B == 1; break; end
    if exist('g_prev', 'var') == 1
        delta = abs(norm(g_prev) - norm(g));
        if DEBUG_LEVEL > 0
            fprintf('%d: delta: %f\n', i, delta);
        end
        if delta < eps; break;
        end
    end
    
    %% S-шаг
    [S e g, phiCoord, w] = s_step(g, S, phiCoord, w);
    
    %% Е-шаг
    [g, g_prev] = e_step(S, g, w, Coords, phiCoord, phiCov);
    
    %% М-шаг
    [w, phiCoord] = m_step(S, e, Coords, phiCoord);
    
    %% Проверка критерия согласия
    if (i > StartChiI)
        [B, Coords, SuspiciousCoords] = chi_square_check( ...
            B, C, SuspiciousCoords, ...
            S, w, Coords, phiCoord, phiCov, phiSigma, coordsA);
    end
end
[Coords SuspiciousCoords] = delete_too_far(S, SuspiciousCoords, ...
    Coords, w, phiCoord, phiSigma);

%% Вывод результатов
if (SHOW_SCENE == 1)
    figure;
    title(sprintf('N = %d', g_N));
    xlabel('x, m');
    ylabel('y, m');
    hold on;
    grid on;
    axis equal;
    scatter(Coords(:,1), Coords(:,2), 100, '+');
    scatter(phiCoord(:,1), phiCoord(:,2), phiSigma, 'o', 'linewidth', 2);
    %for s = 1:S
    %text(phiCoord(s, 1), phiCoord(s, 2), sprintf('\\leftarrow%d', s), ...
    %'color', 'black', 'fontsize', 20);
    %end

    if (~isempty(SuspiciousCoords))
        scatter(SuspiciousCoords(:, 1), SuspiciousCoords(:, 2), 100, '^');
    end
end
